/*
Ardupilot 1.0.1a MEGA Waypoint writer
Use this release to manually upload waypoints
*/
#include "defines.h"

// you can keep your missions stored here, just uncommment the mission to load: (only 1 at a time.)
#include "mission_example.h"
//#include "event_example.h"
//#include "SFO_T3.h"
//#include "SFO_landMe.h"
//#include "Basic_mission_example.h"


// You DON'T need to edit below this line
// ----------------------------------
#include <avr/io.h>
#include <avr/eeprom.h>

// 3D Location vectors
// -------------------
struct Location {
	byte id;				// command id
	byte p1;				// param 1
	long lat;				// Lattitude * 10**7
	long lng;				// Longitude * 10**7
	long alt;				// Altitude in centimeters (meters * 100)
};

const long t7 = 10000000;	// used to scale GPS values for EEPROM storage
byte wp_total;

void setup()
{
	Serial.begin(38400);
	delay(1000);
	Serial.println("\n");
	Serial.println("Waypoint writer 1.0.3 Public Alpha");
	writePoints();  	// saves Waypoint Array


	// output
	eeprom_busy_wait();
	byte options = eeprom_read_byte((uint8_t *)	EE_CONFIG);
	eeprom_busy_wait();
	int32_t hold = eeprom_read_dword((uint32_t *)	EE_ALT_HOLD_HOME);
	eeprom_busy_wait();
	byte radius = eeprom_read_byte((uint8_t *)	EE_WP_RADIUS);
	eeprom_busy_wait();
	byte loiter_radius = eeprom_read_byte((uint8_t *) EE_LOITER_RADIUS);

	delay(1000);

	Serial.println(" ");
	
	if(options & HOLD_ALT_ABOVE_HOME){
		Serial.print("Hold this altitude over home: ");
		Serial.print(hold/100,DEC);
		Serial.println(" meters");
	}else{
		Serial.print("Hold current altitude above home after RTL.");
	}
	Serial.print("WP Radius: ");
	Serial.print(radius ,DEC);
	Serial.println(" meters");
	Serial.print("Loiter Radius: ");
	Serial.print(loiter_radius ,DEC);
	Serial.println(" meters");
	Serial.print("total # of commands including Home:");
	Serial.println(wp_total ,DEC);
	Serial.println(" ");
	readPoints();
}

void loop()
{

}